#include "trianglesurface.h"
#include <smmintrin.h>
#include <assert.h>


#if WALD
// ================================================================================================
// Calculate the optimized triangle
// ================================================================================================
TriangleAcc TriangleSurface::CalcOptimized() {
    TriangleAcc triaccel;
    Vector4 b = triangle.v2 - triangle.v0;
    Vector4 c = triangle.v1 - triangle.v0;
    triaccel.normal = CrossProduct(c,b);

    Vector4 kvec = GetProjectedVerts(triaccel.normal);
    uint k = uint(kvec[3]);

    uint u = (k + 1) % 3;
    uint v = (k + 2) % 3;

    // -- compute the normal k and the greatest dimension
    float nk = 1.0f/DotProduct(triaccel.normal, kvec);
    Vector4 normoverk = triaccel.normal * nk;
    triaccel.n_u = normoverk[u];
    triaccel.n_v = normoverk[v];
    triaccel.n_d = DotProduct(normoverk, triangle.v0);
    triaccel.k = k;

    // -- compute const coefficients for beta
    float denom = 1.0f/(b[u] * c[v] - b[v] * c[u]);
    triaccel.b_nu = -b[v] * denom; 
    triaccel.b_nv = b[u] * denom;
    triaccel.b_d = (b[v] * triangle.v0[u] - b[u] * triangle.v0[v]) * denom;

    // -- compute const coefficients for gamma
    triaccel.c_nu = c[v] * denom;
    triaccel.c_nv = -c[u] * denom;
    triaccel.c_d = (-c[v] * triangle.v0[u] + c[u] * triangle.v0[v]) * denom;

    return triaccel;
}

#endif

#if HAVEL
TriangleAcc TriangleSurface::CalcOptimized() {
    TriangleAcc triaccel;
    Vector4 edge1 = triangle.v2 - triangle.v0;
    Vector4 edge0 = triangle.v1 - triangle.v0;
    Vector4 normal = CrossProduct(edge0, edge1);

    float d = DotProduct(triangle.v0, normal);
    float normmag = 1.0f/Magnitude(normal); 
    normmag *= normmag;
    Vector4 norm1 = CrossProduct(edge1, normal) * normmag;
    Vector4 norm2 = CrossProduct(normal, edge0) * normmag;
    float d1 = -DotProduct(norm1, triangle.v0);
    float d2 = -DotProduct(norm2, triangle.v0);

    triaccel.nx = normal[0];
    triaccel.ny = normal[1];
    triaccel.nz = normal[2];
    triaccel.nd = d;
    triaccel.ux = norm1[0];
    triaccel.uy = norm1[1];
    triaccel.uz = norm1[2];
    triaccel.ud = d1;
    triaccel.vx = norm2[0];
    triaccel.vy = norm2[1];
    triaccel.vz = norm2[2];
    triaccel.vd = d2;

    return triaccel;
};


TriangleAccDP TriangleSurface::CalcOptimized4x4() {
    TriangleAccDP triaccel;
    Vector4 edge1 = triangle.v2 - triangle.v0;
    Vector4 edge0 = triangle.v1 - triangle.v0;
    Vector4 normal = CrossProduct(edge0, edge1);

    float d = DotProduct(triangle.v0, normal);
    float normmag = 1.0f/Magnitude(normal); 
    normmag *= normmag;
    Vector4 norm1 = CrossProduct(edge1, normal) * normmag;
    Vector4 norm2 = CrossProduct(normal, edge0) * normmag;
    float d1 = -DotProduct(norm1, triangle.v0);
    float d2 = -DotProduct(norm2, triangle.v0);

    for(uint i = 0; i < 4; ++i) {
        triaccel.nx[i] = normal[0];
    	triaccel.ny[i] = normal[1];
    	triaccel.nz[i] = normal[2];
    	triaccel.nd[i] = d;
    	triaccel.ux[i] = norm1[0];
    	triaccel.uy[i] = norm1[1];
    	triaccel.uz[i] = norm1[2];
    	triaccel.ud[i] = d1;
    	triaccel.vx[i] = norm2[0];
    	triaccel.vy[i] = norm2[1];
    	triaccel.vz[i] = norm2[2];
    	triaccel.vd[i] = d2;
    }

    return triaccel;
};

#endif

// ================================================================================================
// Transformation function that transforms it's points with the given matrice
// ================================================================================================
void TriangleSurface::Transform(Matrix4x4& mat) {
    triangle.v0 *= mat;
    triangle.v1 *= mat;
    triangle.v2 *= mat;
}